/*  --------------------------- (C) COPYRIGHT 2021 Fortiortech ShenZhen -----------------------------
    File Name      : MotorControlFunction.c
    Author         : Fortiortech  Appliction Team
    Version        : V1.0
    Date           : 2021-11-07
    Description    : This file contains .C file function used for Motor Control.
    ----------------------------------------------------------------------------------------------------
                                       All Rights Reserved
    ------------------------------------------------------------------------------------------------- */
#include <MyProject.h>

CurrentOffset      xdata mcCurOffset;

/*  -------------------------------------------------------------------------------------------------
    Function Name  : FOC_Init
    Description    : mcInit状态下，对FOC的相关寄存器进行配置,先清理寄存器，后配置，最后使能
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void FOC_Init(void)
{
    DRV_CMR = 0x0abf;  //UH/VH/WH UL/VL/WL 互补
    /*使能FOC*/
    ClrBit(DRV_CR, FOCEN);
    SetBit(DRV_CR, FOCEN);
	  SetBit(FOC_CR0, ESCMS);
	  SetBit(FOC_CR3, MFP_EN);
    SetBit(FOC_CR0, MERRS1);
    SetBit(FOC_CR0, MERRS0);
    FOC_EOMEKLPF    = 0xff;
    /*配置FOC寄存器*/
    FOC_CR1         = 0;                                    // 清零 FOC_CR1
    FOC_CR2         = 0;                                    // 清零 FOC_CR2
    FOC_IDREF       = 0;                                    // 清零 Id
    FOC_IQREF       = 0;                                    // 清零 Iq
    FOC__THETA      = 0;                                    // 清零 角度
    FOC_RTHEACC     = 0;                                    // 清零 爬坡函数的初始加速度
    FOC__RTHESTEP   = 0;                                    // 清零 爬坡速度
    FOC_RTHECNT    = 0;                                    // 清零 爬坡次数
    FOC_THECOMP    = _Q15(0.0 / 180.0);                    // SMO 估算补偿角
    FOC_THECOR     = 0x00;//0x04;                                 // 误差角度补偿
	  FOC_ID_LPFK     = 0xff;
    FOC_IQ_LPFK     = 0xff;
    /*电流环参数配置*/
    FOC_DMAX        = DOUTMAX;
    FOC_DMIN        = DOUTMIN;
    FOC_QMAX        = QOUTMAX;
    FOC_QMIN        = QOUTMIN;
    /*位置估算参数配置*/
    FOC_EK1         = OBS_K1T;
    FOC_EK2         = OBS_K2T;
    FOC_EK3         = OBS_K3T;
    FOC_EK4         = 0;//OBS_K4T;
    FOC_FBASE       = OBS_FBASE;
    FOC_OMEKLPF     = SPEED_KLPF;
    FOC_TGLI        = 0;//PWM_TGLI_LOAD;
    /*********PLL或SMO**********/
    #if (EstimateAlgorithm == SMO)
    {
        ClrBit(FOC_CR2, ESEL);
        FOC_KSLIDE    = OBS_KSLIDE;
        FOC_EKLPFMIN  = OBS_EA_KS;
    }
    #elif (EstimateAlgorithm == PLL)
    {
        SetBit(FOC_CR2, ESEL);
        FOC_KSLIDE    = OBSE_PLLKP_GAIN;
        FOC_EKLPFMIN  = OBSE_PLLKI_GAIN;
    }
    #endif //end SVPMW_Mode
    SetBit(FOC_CR1, SVPWMEN);                               // SVPWM模式
    
    if (mcFRState.TargetFR == CW)
    {
        ClrBit(DRV_CR, DDIR);                               // 反转标志位
    }
    else
    {
        SetBit(DRV_CR, DDIR);                               // 反转标志位
    }
    
    /**过调制**/
    #if (OverModulation == 1)
    {
        SetBit(FOC_CR1, OVMDL);                             // 过调制
    }
    #endif //end OverModulation
    //    SetBit(FOC_CR0, UCSEL);
    /*单电阻采样；需要最小采样窗,FOC_TRGDLY为0，七段式SVPWM方式*/
    #if (Shunt_Resistor_Mode == Single_Resistor)
    {
        SetReg(FOC_CR1, CSM0 | CSM1, 0x00);
        FOC_TSMIN  = PWM_TS_LOAD;                           // 最小采样窗口
        FOC_TRGDLY = 0x0F;                                  // 采样时刻在中点，一般考虑开关噪声影响，会设置延迟；
        // 0x0c表示延迟12个clock，提前用反码形式，如0x84表示提前12个clock。
        ClrBit(FOC_CR2, F5SEG);                             // 7段式
        SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, 0x00);
    }
    /*双电阻采样，可设置死区补偿值，在下降沿结束前开始采样Ia，配置81*/
    #elif (Shunt_Resistor_Mode == Double_Resistor)          // double resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0);
        FOC_TSMIN = PWM_DT_LOAD;                            // 死区补偿值
        FOC_TRGDLY = 0x06;                                  // ADC采样的时刻，采样时刻在计数器零点附近，83为下降沿结束前3个clock采样Ia，与单电阻不同
        // 01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        FOC_TBLO = PWM_DLOWL_TIME;                          //下桥臂最小脉冲，保证采样
        SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, 0x00);
        /*五段式或七段式选择*/
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG);                         // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG);                         // 5段式
        }
        #endif
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        {
            ClrBit(FOC_CR2, DSS);                           // 7段式
        }
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        {
            SetBit(FOC_CR2, DSS);                           // 5段式
        }
        #endif //end DouRes_Sample_Mode
    }
    /*三电阻采样*/
    #elif (Shunt_Resistor_Mode == Three_Resistor)           // signel resistor sample
    {
        SetReg(FOC_CR1, CSM0 | CSM1, CSM0 | CSM1);          // 三电阻
        FOC_TSMIN  = PWM_DT_LOAD;                           // 死区补偿值
        FOC_TRGDLY = 0x06;                                  // ADC采样的时刻，采样时刻在计数器零点附近，83为下降沿结束前3个clock采样Ia，与单电阻不同。
        // 01为上升沿开始后第一个clock开始采样。根据实际情况调整。
        SetReg(CMP_CR1, CMP3MOD0 | CMP3MOD1, CMP3MOD0 | CMP3MOD1);
        FOC_TBLO = PWM_OVERMODULE_TIME;                     // 过调制电流采样处理的TB脉宽
        /*五段式或七段式选择*/
        #if (SVPMW_Mode == SVPWM_7_Segment)
        {
            ClrBit(FOC_CR2, F5SEG);                         // 7段式
        }
        #elif (SVPMW_Mode == SVPWM_5_Segment)
        {
            SetBit(FOC_CR2, F5SEG);                         // 5段式
        }
        #endif //end SVPMW_Mode
        #if (DouRes_Sample_Mode == DouRes_1_Cycle)
        {
            ClrBit(FOC_CR2, DSS);                           // 7段式
        }
        #elif (DouRes_Sample_Mode == DouRes_2_Cycle)
        {
            SetBit(FOC_CR2, DSS);                           // 5段式
        }
        #endif //end DouRes_Sample_Mode
    }
    #endif  //end Shunt_Resistor_Mode
    /* 使能电流基准校正 */
    #if (CalibENDIS == Enable)
    {
        if (mcCurOffset.OffsetFlag == 1)
        {
            #if (Shunt_Resistor_Mode == Single_Resistor)    // 单电阻校正
            {
                /*set ibus current sample offset*/
                SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
                FOC_CSO = mcCurOffset.Iw_busOffset;         // 写入Ibus的偏置
            }
            #elif (Shunt_Resistor_Mode == Double_Resistor)  // 双电阻校正
            {
                /*set ia, ib current sample offset*/
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
                FOC_CSO  = mcCurOffset.IuOffset;            // 写入IA的偏置
            
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
                FOC_CSO  = mcCurOffset.IvOffset;            // 写入IB的偏置
            }
            #elif (Shunt_Resistor_Mode == Three_Resistor)   // 三电阻校正
            {
                /*set ibus current sample offset*/
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC0);
                FOC_CSO = mcCurOffset.IuOffset;             // 写入IA的偏置
            
                SetReg(FOC_CR2, CSOC0 | CSOC1, CSOC1);
                FOC_CSO = mcCurOffset.IvOffset;             // 写入IB的偏置
            
                SetReg(FOC_CR2, CSOC0 | CSOC1, 0x00);
                FOC_CSO = mcCurOffset.Iw_busOffset;         // 写入IC的偏置
            }
            #endif  //end Shunt_Resistor_Mode
        }
    }
    #endif  //end CalibENDIS
    /*  -------------------------------------------------------------------------------------------------
        DRV_CTL：PWM来源选择
        OCS = 0, DRV_COMR
        OCS = 1, FOC/SVPWM/SPWM
        -------------------------------------------------------------------------------------------------*/
    /*计数器比较值来源FOC*/
    SetBit(DRV_CR, OCS);
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Motor_Charge
    Description    : 预充电，当一直处于预充电状态下，不接电机，可用于验证IPM或者Mos。
                     预充电分三步，第一步是对U相进行预充电，第二步是对U,V两相进行预充电;第三步是对U、V、W三相进行预充电。
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Motor_Charge(void)
{
    if (McStaSet.SetFlag.ChargeSetFlag == 0)
    {
        McStaSet.SetFlag.ChargeSetFlag = 1;
        DRV_DR = 0.1 * DRV_ARR;              //下桥臂10% duty
        /*  -------------------------------------------------------------------------------------------------
            DRV_CTL：PWM来源选择
            OCS = 0, DRV_COMR
            OCS = 1, FOC/SVPWM/SPWM
            -------------------------------------------------------------------------------------------------*/
        ClrBit(DRV_CR, OCS);
        mcFocCtrl.ChargeStep = 0;
        //        DRV_CMR &= 0xFFc0;
    }
    
    if ((mcFocCtrl.State_Count < Charge_Time) && (mcFocCtrl.ChargeStep == 0))
    {
        mcFocCtrl.ChargeStep = 2;
        #if (IPMState == IPMtest)
        {
            DRV_CMR |= 0x30;                         // U相输出
        }
        #elif (IPMState == NormalRun)                 // 正常按电机状态机运行
        {
            DRV_CMR = 0x01;                         // U相下桥臂通
        }
        #endif
        MOE = 1;	//输出使能
    }
    
    if (( mcFocCtrl.State_Count <= (Charge_Time << 1) / 3) && (mcFocCtrl.ChargeStep == 1))
    {
        mcFocCtrl.ChargeStep = 2;
        #if (IPMState == IPMtest)
        {
            DRV_CMR |= 0x0F;                         // U、V相输出
        }
        #elif (IPMState == NormalRun)                 // 正常按电机状态机运行
        {
            DRV_CMR = 0x04;                         // V相下桥臂导通
        }
        #endif
    }
    
    if ((mcFocCtrl.State_Count <= Charge_Time / 3) && (mcFocCtrl.ChargeStep == 2))
    {
        mcFocCtrl.ChargeStep = 3;
        #if (IPMState == IPMtest)
        {
					   
            DRV_CMR |= 0x3F;                         // U、V、W相输出
        }
        #elif (IPMState == NormalRun)                 // 正常按电机状态机运行
        {
            DRV_CMR = 0x10;                         // W相下桥臂导通
        }
        #endif
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Motor_Align
    Description    : 预定位函数，当无逆风判断时，采用预定位固定初始位置;当有逆风判断时，采用预定位刹车
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Motor_Align(void)
{
    if (McStaSet.SetFlag.AlignSetFlag == 0)
    {
        McStaSet.SetFlag.AlignSetFlag = 1;
        /* -----FOC初始化----- */
        FOC_Init();
        /*配置预定位的电流、KP、KI*/
        FOC_IDREF = ID_Align_CURRENT;
        FOC_IQREF = IQ_Align_CURRENT;
        FOC_DKP  = DQKP_Alignment;
        FOC_DKI  = DQKI_Alignment;
        FOC_QKP  = DQKP_Alignment;
        FOC_QKI  = DQKI_Alignment;
        FOC_EKP   = OBSW_KP_GAIN;
        FOC_EKI   = OBSW_KI_GAIN;
        /*配置预定位角度*/
        FOC__THETA  = Align_Theta;
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO)
        {
            FOC__ETHETA   = FOC__THETA - 4096;
        }
        #elif (EstimateAlgorithm == PLL)
        {
            FOC__ETHETA   = FOC__THETA;
        }
        #endif //end    EstimateAlgorithm
        /*使能输出*/
             DRV_CMR |= 0x3F;                         // U、V、W相输出
        MOE = 1;
        //MOE = 1;
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Motor_Open
    Description    : 开环启动的参数配置
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Motor_Open(void)
{
	static uint8 OpenRampCycles;
    mcFocCtrl.FGDelayOutputTimer = 0;
    
    if (McStaSet.SetFlag.StartSetFlag == 0)
    {
        McStaSet.SetFlag.StartSetFlag = 1;
    }
    
    FOC_Init();
    /****启动初始角度赋值****/
    //    #if (PosCheckEnable==1)                             //初始位置检测不使能时初始角度为预定位角度
    //    FOC__THETA  = RPDPara.ThetaGet;              // 无初始位置检测，则用预定位角
    //    FOC__ETHETA = FOC__THETA ;
    //    #elif (PosCheckEnable==0)                               //初始位置检测不使能时初始角度为预定位角度
    FOC__THETA  = Align_Theta;              // 无初始位置检测，则用预定位角
		FOC__EOME = 0;
		
    /*启动电流、KP、KI、FOC_EKP、FOC_EKI*/
    FOC_IDREF = ID_Start_CURRENT;                         // D轴启动电流
    mcFocCtrl.mcIqref = IQ_Start_CURRENT;                 // Q轴启动电流
    FOC_DKP = DKP_START;
    FOC_DKI = DKI_START;
    FOC_QKP = QKP_START;
    FOC_QKI = QKI_START;
    FOC_EKP  = OBSW_KP_GAIN;
    FOC_EKI  = OBSW_KI_GAIN;
    /*启动方式选择*/
    #if (Open_Start_Mode == Omega_Start)              // Omega 启动
    {
        FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
        FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
        FOC_EFREQHOLD = Motor_Omega_Ramp_End;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | ANGM);
    }
    #elif (Open_Start_Mode == Open_Start)
    {
        FOC_RTHEACC      = Motor_Open_Ramp_ACC;      // 爬坡函数的初始加速度
        FOC__RTHESTEP    = Motor_Open_Ramp_Min;      // 0.62 degree acce speed
        FOC_RTHECNT     = MOTOR_OPEN_ACC_CNT;       // acce time
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, RFAE);
			
			  
    }
    #elif (Open_Start_Mode == Open_Omega_Start)
    {
        FOC_RTHEACC   = Motor_Open_Ramp_ACC;         // 爬坡函数的初始加速度
        FOC__RTHESTEP = Motor_Open_Ramp_Min;         // 0.62 degree acce speed
        FOC_RTHECNT  = MOTOR_OPEN_ACC_CNT;          // acce time
        FOC_EFREQACC  = Motor_Omega_Ramp_ACC;
        FOC_EFREQMIN  = Motor_Omega_Ramp_Min;
        FOC_EFREQHOLD = Motor_Omega_Ramp_End;
        SetReg(FOC_CR1, EFAE | RFAE | ANGM, EFAE | RFAE | ANGM);
    }
    #endif //end Open_Start_Mode
    /*不同启动方式下，切换到MCRUN状态*/
    #if (Open_Start_Mode == Open_Start)         //OPEN状态启动时拖动多次
    {
        if (OpenRampCycles < (MOTOR_OPEN_ACC_CYCLE - 1))
        {
            if (!ReadBit(FOC_CR1, RFAE))
            {
                SetBit(FOC_CR1, RFAE);
                OpenRampCycles++;
            }
        }
        else
        {
            mcFocCtrl.State_Count = 2;
            mcState = mcRun;
        }
        
        FOC_EKP = OBSW_KP_GAIN_RUN4;                            // 估算器里的PI的KP
        FOC_EKI = OBSW_KI_GAIN_RUN4;                            // 估算器里的PI的KI
    }
    #elif (Open_Start_Mode == Open_Omega_Start)
    {
        mcFocCtrl.State_Count = 2600;
        mcState = mcRun;
    }
    #elif (Open_Start_Mode == Omega_Start)
    {
        /*********PLL或SMO**********/
        #if (EstimateAlgorithm == SMO)
        {
            mcFocCtrl.State_Count = 2500;
        }
        #elif (EstimateAlgorithm == PLL)
        {
            FOC_EKP = OBSW_KP_GAIN_RUN5;                       // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN5;                       // 估算器里的PI的KI
        }
        #endif //end    EstimateAlgorithm
        mcState   = mcRun;
    }
    #endif //end Open_Start_Mode
    FOC_IQREF = mcFocCtrl.mcIqref;                          // Q轴启动电流
		 /*使能输出*/
             DRV_CMR |= 0x3F;                         // U、V、W相输出
        MOE = 1;	//主输出使能
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Motor_TailWind
    Description    : 顺风逆风参数配置函数
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Motor_TailWind(void)
{
    if (mcFocCtrl.State_Count == 0)
    {
        if (McStaSet.SetFlag.TailWindSetFlag == 0) //初始化
        {
            McStaSet.SetFlag.TailWindSetFlag = 1;
            #if (FRDetectMethod == RSDMethod)
            {
                RSDDetectInit();
            }
            #elif (FRDetectMethod == FOCMethod)
            {
                TailWindDetectInit();
            }
            #elif (FRDetectMethod == BEMFMethod)
            {
                BEMFDetectInit();
            }
            #endif
        }
        
        #if (FRDetectMethod == RSDMethod)
        {
            RSDDealwith();
        }
        #elif (FRDetectMethod == FOCMethod)
        {
            FOCTailWindDealwith();
        }
        #elif (FRDetectMethod==BEMFMethod)
        {
            BEMFDealwith();
        }
        #endif
        
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : MC_Stop
    Description    : inital motor control parameter
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void MC_Stop(void)
{
    #if (!StartONOFF_Enable)
    {
        #if (StopBrakeFlag == 0)
        {
            mcState = mcReady;
            FOC_CR1 = 0x00;
            ClrBit(DRV_CR, FOCEN);   //关闭FOC
            MOE = 0;
            mcFocCtrl.FGDelayOutputTimer = 0;
        }
        #else
        {
            MOE = 0;
            FOC_CR1 = 0x00;
            ClrBit(DRV_CR, FOCEN);
            DRV_DR   = 0.8 * (DRV_ARR ) + 1;
            DRV_CMR  = 0x00;
            DRV_CMR |= 0x015;     // 三相下桥臂通，刹车
            ClrBit(DRV_CR, OCS);  // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
            MOE = 1;
            mcState  = mcBrake;
            mcFocCtrl.State_Count = StopWaitTime;
        }
        #endif
    }
    #else
    {
        if (mcFocCtrl.SpeedFlt < Motor_Stop_Speed)
        {
            MOE = 0;
            FOC_CR1 = 0x00;
            ClrBit(DRV_CR, FOCEN);
//            DRV_DR   = DRV_ARR + 1;
//            DRV_CMR  = 0x00;
//            DRV_CMR |= 0x015;     // 三相下桥臂通，刹车
//            ClrBit(DRV_CR, OCS);  // OCS = 0, DRV_COMR;OCS = 1, FOC/SVPWM/SPWM
//            MOE = 1;
            mcState  = mcBrake;
            mcFocCtrl.State_Count = StopWaitTime1;
        }
    }
    #endif
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : MC_Break
    Description    : inital motor control parameter
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void MC_Break(void)
{
	if (mcFocCtrl.State_Count == 0)
	{
		mcState = mcReady;
		MOE = 0;
		ClrBit(DRV_CR, FOCEN);
	}
}
/*  -------------------------------------------------------------------------------------------------
    Function Name  : MotorcontrolInit
    Description    : 控制变量初始化清零,包括保护参数的初始化、电机状态初始化
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void MotorcontrolInit(void)
{
    /* -----保护参数初始化----- */
    memset(&mcFaultDect, 0, sizeof(FaultVarible));        // FaultVarible变量清零
    /* -----保护次数初始化----- */
    memset(&mcProtectTime, 0, sizeof(ProtectVarible));    // ProtectVarible保护次数清零
    /* -----过流保护----- */
    memset(&mcCurVarible, 0, sizeof(CurrentVarible));     // 电流保护的变量清零
    /* -----顺逆风判断参数初始化----- */
    //    memset(&BEMFDetect , 0 , sizeof(BEMFDetect_TypeDef));//  BEMFDetect所有变量清零
    /* -----启停测试参数初始化----- */
    memset(&ONOFFTest, 0, sizeof(ONVarible));
    /*****电机状态机时序变量***********/
    McStaSet.SetMode                   = 0;
	
	
    /**********电机目标方向*************/
	if(FRPin==0)
	{
		mcFRState.TargetFR                 = CW;
	}
	else if(FRPin==1)
	{
		mcFRState.TargetFR                 = CCW;
	}
	
//    #if (IRMODE == 0)
//    {
//        mcFRState.TargetFR                 = CW;
//    }
//    #else
//    {
//        mcFRState.TargetFR                 = CCW;
//    }
//    #endif //end IRMODE
    /* -----外部控制环----- */
    memset(&mcFocCtrl, 0, sizeof(FOCCTRL));             // mcFocCtrl变量清零
    /* -----ADC采样滤波值----- */
    memset(&AdcSampleValue, 0, sizeof(ADCSample));      // ADCSample变量清零
    /* -----电流偏置校准变量初始化----- */
    memset(&mcCurOffset, 0, sizeof(CurrentOffset));    // mcCurOffset变量清零
    mcCurOffset.IuOffsetSum            = 16383;
    mcCurOffset.IvOffsetSum            = 16383;
    mcCurOffset.Iw_busOffsetSum        = 16383;
    /* -----速度环响应参数初始化----- */
    memset(&MotorControl, 0, sizeof(MCRAMP));          // mcSpeedRamp变量清零
    /* -----PWM调速变量初始化----- */
    memset(&mcPwmInput, 0, sizeof(PWMINPUTCAL));      // mcPwmInput变量清零
    mcPwmInput.PwmArr = 65535;
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : VariablesPreInit
    Description    : 初始化电机参数
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void VariablesPreInit(void)
{
    mcFaultSource = 0;
    /* -----保护参数初始化----- */
    memset(&mcFaultDect, 0, sizeof(FaultVarible));                                                                 // FaultVarible变量清零
    /* -----外部控制环参数初始化----- */
    memset(&mcFocCtrl, 0, sizeof(FOCCTRL));                                                                // mcFocCtrl变量清零
    /* -----顺逆风参数初始化----- */
    memset(&Stk, 0, sizeof(Stk_TypeDef));		//反电动势变量清零
    Stk.BemfFR                         = 0xFF;
    Stk.Calcnum                        = 10;
    BEMFDetect.BEMFBrakeFlag           = 0;
    //    memset(&RPDPara, 0, sizeof(RPD_Param_TypeDef));
    //    RPDPara.NextShotFlag = 1;
    //    RPDPara.InjectOffFocIntCnt = Inject_Off_Foc_Int_Cnt;
    //    RPDPara.InjectOnFocIntCnt = Inject_On_Foc_Int_Cnt;
    //    RPDPara.InjectStep1VoltageProportion = Inject_Step1_Voltage_Proportion;
    //    RPDPara.InjectStep2VoltageProportion = Inject_Step2_Voltage_Proportion;
    /*****电机状态机时序变量***********/
    McStaSet.SetMode                  = 0x01;   //电流校准标志位置1，其它置0
	  memset(&Time, 0, sizeof(TIMERTypeDef));
    mcFocCtrl.State_Count = 0;
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : GetCurrentOffset
    Description    : 上电时，先对硬件电路的电流进行采集，写入对应的校准寄存器中。
                     调试时，需观察mcCurOffset结构体中对应变量是否在范围内。采集结束后，OffsetFlag置1。
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void GetCurrentOffset(void)
{
    SetBit(ADC_CR, ADCBSY);             // 使能ADC
    
    while (ReadBit(ADC_CR, ADCBSY));
    
    #if (Shunt_Resistor_Mode == Single_Resistor)                   //单电阻模式
    {
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset     = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
    }
    #elif (Shunt_Resistor_Mode == Double_Resistor)                 //双电阻模式
    {
        mcCurOffset.IuOffsetSum     += ((ADC0_DR & 0x7ff8));		//U相电流采样
        mcCurOffset.IuOffset         = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum     -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum     += ((ADC1_DR & 0x7ff8));		//V相电流采样
        mcCurOffset.IvOffset         = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum     -= mcCurOffset.IvOffset;
    }
    #elif (Shunt_Resistor_Mode == Three_Resistor)                 //三电阻模式
    {
        mcCurOffset.IuOffsetSum     += ((ADC0_DR & 0x7ff8));
        mcCurOffset.IuOffset         = mcCurOffset.IuOffsetSum >> 4;
        mcCurOffset.IuOffsetSum     -= mcCurOffset.IuOffset;
        mcCurOffset.IvOffsetSum     += ((ADC1_DR & 0x7ff8));
        mcCurOffset.IvOffset         = mcCurOffset.IvOffsetSum >> 4;
        mcCurOffset.IvOffsetSum     -= mcCurOffset.IvOffset;
        mcCurOffset.Iw_busOffsetSum += ((ADC4_DR & 0x7ff8));
        mcCurOffset.Iw_busOffset     = mcCurOffset.Iw_busOffsetSum >> 4;
        mcCurOffset.Iw_busOffsetSum -= mcCurOffset.Iw_busOffset;
    }
    #endif
    mcCurOffset.OffsetCount++;
    
    if (mcCurOffset.OffsetCount > Calib_Time)
    {
        mcCurOffset.OffsetFlag = 1;
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Motor_Ready
    Description    : 上电时，关闭输出，先对硬件电路的电流进行采集，在FOC_Init中写入对应的校准寄存器中。
                     调试时，需观察mcCurOffset结构体中对应变量是否在范围内。
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Motor_Ready(void)
{
    if (McStaSet.SetFlag.CalibFlag == 0)
    {
        McStaSet.SetFlag.CalibFlag = 1;
        ClrBit(DRV_CR, FOCEN);
        MOE = 0;
        //mcCurOffset.OffsetFlag = 0;
        mcCurOffset.OffsetCount = 0;    //偏置电压采集计数
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Motor_Init
    Description    : 对电机相关变量、PI进行初始化设置
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Motor_Init(void)
{
    VariablesPreInit();                           // 电机相关变量初始化
    PI_Init();                                    // PI初始化
}

